#ifndef CONTROLLER_TRACKER__CONTROLLER_TRACKER_NODE_HPP_
#define CONTROLLER_TRACKER__CONTROLLER_TRACKER_NODE_HPP_

// Eigen
#include <Eigen/Eigen>

// ROS
#include <geometry_msgs/msg/point.hpp>
#include <geometry_msgs/msg/quaternion.hpp>
#include <geometry_msgs/msg/vector3.hpp>
#include <geometry_msgs/msg/transform_stamped.hpp>
#include <geometry_msgs/msg/pose.hpp>
#include <geometry_msgs/msg/pose_stamped.hpp>
#include <message_filters/subscriber.h>
#include <urdf/model.h>
#include <urdf_model/model.h>

#include <tf2_ros/buffer.h>
#include <tf2_ros/create_timer_ros.h>
#include <tf2_ros/message_filter.h>
#include <tf2_ros/transform_listener.h>
#include <tf2_ros/transform_broadcaster.h>
#include <tf2/LinearMath/Quaternion.h>

#include <rclcpp/rclcpp.hpp>
#include <std_srvs/srv/trigger.hpp>
#include <tf2_geometry_msgs/tf2_geometry_msgs.hpp>
#include <visualization_msgs/msg/marker_array.hpp>
#include <apriltag_msgs/msg/april_tag_detection.hpp>
#include <apriltag_msgs/msg/april_tag_detection_array.hpp>

// STD
#include <memory>
#include <string>
#include <vector>



namespace controller_tracker {

using AprilTagDetectionArray = apriltag_msgs::msg::AprilTagDetectionArray;

class ControllerTrackerNode : public rclcpp::Node {
public:
  explicit ControllerTrackerNode(const rclcpp::NodeOptions & options);

private:
  void tagCallback(const AprilTagDetectionArray::SharedPtr msg); // 处理订阅到的消息

  void loadTagPositions();

  std::shared_ptr<tf2_ros::Buffer> tf_buffer_;
  std::shared_ptr<tf2_ros::TransformListener> tf_listener_;

  // std::map<std::string, rclcpp::Parameter> tags_positions_;


  urdf::Model model_;
  std::string urdf_path_;

  std::vector<int64_t> tag_ids_;
  std::vector<std::vector<double>> positions_;
  std::vector<std::vector<double>> orientations_;

  


  rclcpp::Subscription<AprilTagDetectionArray>::SharedPtr sub_;
  rclcpp::Publisher<geometry_msgs::msg::PoseStamped>::SharedPtr target_pose_pub_;
};

}   // namespace controller_tracker

#endif  // CONTROLLER_TRACKER__CONTROLLER_TRACKER_NODE_HPP_